/**
 * @FilePath: \ros2\src\agv_control\haredware\src\hardware.cpp
 * @Date: 2025-03-13 08:09:30
 * @Description:  
 * @Author: haibo
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-20 19:15:50
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#include <limits>
#include <vector>

#include "hardware/hardware.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
#include <shared_parameters/shared_params_interface.hpp>
#include "motor/motor_factory.h"
#include "encoder/encoder_factory.h"
#include "agv_msgs/msg/agv_event.hpp"

/*************************************************
 *       Hardware interface implementation       *
 *************************************************/
namespace hardware
{
  
/**
 * @brief 初始化硬件设备
 *
 * 根据传入的设备名称和索引初始化硬件设备。根据制造商代码决定初始化电机还是编码器。
 *
 * @param name 设备名称
 * @param index 设备索引
 * @return 初始化成功返回true，否则返回false
 */
bool Hardware::init_hardware(const std::string name, const int index) 
{
  int nodeid = -1;
  int manufacturer = 0;

  std::string manufacturer_name = name + "/manufacturer";
  this->get_parameter_or(manufacturer_name, manufacturer, 0);
  std::string node_id_name = name + "/nodeId";
  this->get_parameter_or(node_id_name, nodeid, -1);
  

  if (manufacturer < 20) {
    std::string start_seq;
    this->get_parameter_or(name + "/startSeq", start_seq, std::string(""));

    std::shared_ptr<ros_canopen_driver::Motor> s = 
      ros_canopen_driver::MotorFactory::generateMotor(shared_from_this(), name, manufacturer);
    s->nodeid = nodeid;
    HWDevice motor(s, name, kMotorDevice, index); 

    if (!start_seq.empty())
      motor.set_seq(start_seq);
    devices_.push_back(motor);
  } else {
    std::shared_ptr<encoder::Encoder> e = 
      encoder::EncoderFactory::generateEncoder(shared_from_this(), name, manufacturer);
    e->init();
    HWDevice encoder(e, name, kEncoderDevice, index);
    devices_.push_back(encoder); 
  }
  RCLCPP_INFO_STREAM(get_logger(), name << " manufacturer code: " << manufacturer);
  return true;
}

/**
 * @brief 设置硬件初始化
 *
 * 该函数用于初始化硬件组件，包括设置默认参数、初始化事件发布器、创建并初始化各种服务和客户端等。
 *
 * 1. 遍历设备列表，如果设备位置为NaN，则重置设备。
 * 2. 初始化事件发布器。
 * 3. 创建并初始化发布器和客户端，包括驱动信息发布器、设置CANopen入口服务客户端、输出接口服务客户端等。
 * 4. 创建并绑定电机启用服务、初始化服务、恢复服务、停止服务和关闭服务的回调函数。
 * 5. 创建并绑定读取输入订阅器，当接收到新的输入时调用回调函数。
 * 6. 在单独的线程中初始化伺服电机。
 * 7. 创建并启动硬件定时器，每隔1秒调用一次回调函数。
 * 8. 在单独的线程中启动多线程执行器，并添加当前节点以进行旋转。
 */
void Hardware::setup()
{
  // set some default values
  for (auto device : devices_) {
    if (std::isnan(device.position)) {
      device.reset();
    }
  }

  event_publisher_.init(shared_from_this());
  drivers_info_publisher_ = this->create_publisher<agv_msgs::msg::DriversInfo>("driversInfo", 1);
  set_entry_client_ = this->create_client<agv_srvs::srv::SetCanopenEntry>("/set_canopen_entry");
  output_client_ = this->create_client<agv_srvs::srv::OutputInterface>("/outputinterface");

  srv_motor_enable_ = this->create_service<agv_srvs::srv::MotorEnable>(
    "motorEnable", 
    std::bind(&Hardware::motor_enable_callback, this, std::placeholders::_1, std::placeholders::_2));

  srv_init_ = this->create_service<std_srvs::srv::Trigger>(
      "driver/init", std::bind(&Hardware::handle_init, this, std::placeholders::_1, std::placeholders::_2));
  srv_recover_ = this->create_service<std_srvs::srv::Trigger>(
      "driver/recover", std::bind(&Hardware::handle_recover, this, std::placeholders::_1, std::placeholders::_2));
  srv_halt_ = this->create_service<std_srvs::srv::Trigger>(
      "driver/halt", std::bind(&Hardware::handle_halt, this, std::placeholders::_1, std::placeholders::_2));
  srv_shutdown_ = this->create_service<std_srvs::srv::Trigger>(
      "driver/shutdown", std::bind(&Hardware::handle_shutdown, this, std::placeholders::_1, std::placeholders::_2));

  readinput_sub_ = this->create_subscription<agv_msgs::msg::ReadInPutsNew>(
      "/readInputNew", 10, std::bind(&Hardware::reset_button_callback, this, std::placeholders::_1));

  std::thread(&Hardware::init_servos, this).detach();
  
  // 50ms
  hardware_timer_ = this->create_wall_timer(
    std::chrono::milliseconds(50), [this]() { this->hardware_timer_callback(); });

  spin_thread_ = std::thread([this]() {
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(this->shared_from_this());
    executor.spin();
  });
}

void Hardware::hardware_timer_callback()
{
  state_publish();
  call_output_service();
}

/**
 * @brief 声明参数
 *
 * 该函数用于声明硬件参数。首先尝试初始化共享内存接口，然后从共享参数中获取节点参数，
 * 并根据参数类型声明对应的参数。此外，还从共享参数中获取IO参数，并将其添加到输出端口列表中。
 *
 * @return 如果参数声明成功，则返回true；否则返回false。
 */
bool Hardware::declare_parameters() 
{
  if (!SharedParamsInterface::initialize()) {
    RCLCPP_ERROR(get_logger(), "Failed to initialize shared memory interface");
    return false;
  }

  try { 
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      RCLCPP_ERROR_STREAM(get_logger(), "Error get shared parameters");
      return false;
    } 

    params = SharedParamsInterface::getNodeParams("joint");
    for (const auto& [name, value] : params) {
      RCLCPP_INFO_STREAM(get_logger(), 
          "joint:" << name << ", value:" << value.toString());
      switch(value.type) {
        case SharedParamsInterface::ParamValue::Type::INT:
            this->declare_parameter(name, value.getValue<int>());
            break;
        case SharedParamsInterface::ParamValue::Type::DOUBLE:
            this->declare_parameter(name, value.getValue<double>());
            break;
        case SharedParamsInterface::ParamValue::Type::STRING:
            this->declare_parameter(name, value.getValue<std::string>());
            break;
        case SharedParamsInterface::ParamValue::Type::BOOL:
            this->declare_parameter(name, value.getValue<bool>());
            break;
        default:
            RCLCPP_WARN_STREAM(get_logger(), 
              "Unsupported parameter type for " << name);
            break;
      }
    }
    
    params = SharedParamsInterface::getNodeParams("IO");
    for (const auto& [name, value] : params) {
      if (name.find("servoPower") != std::string::npos ||
        name.find("brakeOut") != std::string::npos ||
        name.find("liftRelay") != std::string::npos ||
        name.find("preChargeOff") != std::string::npos) {
        
        RCLCPP_INFO_STREAM(get_logger(), 
            "IO:" << name << ", value:" << value.toString());
        
        agv_msgs::msg::OutputPort io;
        io.index = value.getValue<int>();
        io.level = true;  
        outports_.push_back(io);
      }
    }
  } catch (const std::exception& e) {
    RCLCPP_ERROR_STREAM(get_logger(), 
      "Error reading parameters: " << e.what());
    return false;
  }
  return true;
}

/**
 * @brief 读取硬件设备信息
 *
 * 该函数遍历devices_容器中的每个设备，根据设备类型读取相关信息。
 *
 * @return 如果读取成功返回true，否则返回false。
 */
bool Hardware::read() 
{
  bool ret = true;

  for (auto &dev : devices_) {
    // Update the joint status: this is a revolute joint without any limit.
    if (dev.type == kMotorDevice) {
      if (!dev.initialized) {
        RCLCPP_WARN_ONCE(get_logger(), "Read paused. Please wait for device %s to initialize.", dev.name.c_str());
        continue;
      }
      auto motor = dev.getDevice<ros_canopen_driver::Motor>();
      if (motor != nullptr) {
#ifdef SIMULATION_MODE
        // RCLCPP_INFO_THROTTLE(get_logger(), *(get_clock()),
        // 500,
        // "read:%f,%f", dev.velocity_command, dev.position_command);
        dev.position += dev.velocity_command;
        dev.velocity = dev.velocity_command;
        dev.effort = 0; 
#else
        dev.position = motor->Motor_GetCurrentPos();
        dev.velocity = motor->Motor_GetCurrentVel();
        dev.effort = motor->Motor_GetCurrent();
        // dev.timestamp = motor->Motor_GetTimestamp();
#endif
      } else {
        RCLCPP_ERROR_STREAM(get_logger(), "Failed to read motor device: " << dev.name);
        ret = false;
      }
    } else if (dev.type == kEncoderDevice) {
      auto encoder = dev.getDevice<encoder::Encoder>();
      if (encoder != nullptr) {
#ifdef SIMULATION_MODE
        dev.position = 0.0;
#else
        dev.position = encoder->getRadValue();
#endif
        dev.velocity = 0.0;
        dev.effort = 0.0;
        // dev.timestamp = encoder->timestamp_;
      } else {
        RCLCPP_ERROR_STREAM(get_logger(), "Failed to read encoder device: " << dev.name);
        ret = false;
      }
    } else {
      RCLCPP_ERROR(get_logger(), 
        "Get hardware type failed: %s type:%d", dev.name.c_str(), dev.type);
      ret = false;
    }
  }
  return ret;
}

/**
 * @brief 向硬件设备写入数据
 *
 * 遍历所有设备，并根据设备的类型和状态进行不同的处理。
 * 如果设备是编码器设备或者未初始化，则跳过该设备的写入操作。
 * 如果设备是电机设备且已经初始化，则根据设备状态设置速度和位置命令。
 *
 * @return 如果所有设备写入成功，则返回 true；否则返回 false。
 */
bool Hardware::write()
{
  bool ret = true;
  for (auto& dev : devices_) {
    // RCLCPP_INFO_STREAM(get_logger(), "name:" << dev.name << ",init:" << dev.initialized);
    if (dev.type == kEncoderDevice || !dev.initialized) {
      if (!dev.initialized)
        RCLCPP_WARN_ONCE(get_logger(), "Write paused. Please wait for device %s to initialize.", dev.name.c_str());
      continue;
    }
    auto motor = dev.getDevice<ros_canopen_driver::Motor>();
    if (motor != nullptr) {
#ifndef SIMULATION_MODE
      double velocity_command = 0.0;
      double position_command = 0.0;
      if (motor->Motor_GetState() == ros_canopen_driver::MotorState::Ready) {
        velocity_command = dev.velocity_command;
        position_command = dev.position_command;
      } 
      // RCLCPP_INFO_STREAM(get_logger(), "cmd:" << velocity_command);
      motor->Motor_SetTargetVelRad(velocity_command);
      motor->Motor_SetTargetPosRad(position_command);
#else
      static double velocity_command = dev.velocity_command;
      static double position_command = dev.position_command;
      if (velocity_command != dev.velocity_command || position_command != dev.position_command) {
        velocity_command = dev.velocity_command;
        position_command = dev.position_command;
        RCLCPP_INFO_STREAM(get_logger(), "[" << dev.name << "]velocity cmd:" << dev.velocity_command);
        RCLCPP_INFO_STREAM(get_logger(), "[" << dev.name << "]position cmd:" << dev.position_command);
      }
#endif
    } else {
       RCLCPP_ERROR_STREAM(get_logger(), "Failed to write motor device: " << dev.name);
      ret = false;
    }
  }
  return ret;
}

bool Hardware::motor_enable_callback(const std::shared_ptr<agv_srvs::srv::MotorEnable::Request> req, 
    std::shared_ptr<agv_srvs::srv::MotorEnable::Response> res) 
{
  std::vector<std::future<bool>> futures;
  futures.resize(0);

  for (auto n : req->joint_names) {
    bool find = false;
    for (auto dev : devices_) {
      if (n.compare(dev.name) == 0) {
        find = true;
        RCLCPP_INFO_STREAM(get_logger(), "Motor enable callback, enable device name:" << n);
        auto motor = dev.getDevice<ros_canopen_driver::Motor>();
        if (motor) {
            if (req->enable) {
#ifdef SIMULATION_MODE
              enable_motor_ = true;
#else
              std::future<bool> fut = std::async(std::launch::async, std::bind(&ros_canopen_driver::Motor::Motor_Recover, motor));
              futures.emplace_back(std::move(fut));
#endif
            } else {
#ifdef SIMULATION_MODE
              enable_motor_ = false;
#else
              std::future<bool> fut = std::async(std::launch::async, std::bind(&ros_canopen_driver::Motor::Motor_Halt, motor));
              futures.emplace_back(std::move(fut)); 
#endif
            }
        } else {
          find = false;
        }
      }
    }

    if (!find) {
      RCLCPP_WARN(get_logger(), "[Hardware] enable_srv: cannot find joint name: %s", n.c_str());
      res->success = false;
      return false;
    }
  }

  bool result = false;
  bool finished = true;
#ifdef SIMULATION_MODE
  res->success = true;  
#else
  for (int i=0; i< futures.size(); i++) {
    try {
      result = futures[i].get();
    } catch (const std::future_error& e) {
      RCLCPP_ERROR_STREAM(get_logger(), "[Hardware] future " << i << " get result error: " << e.what());
      finished = false;
    }

    if (!result) {
      RCLCPP_ERROR_STREAM(get_logger(), "servo enable/disable error! " << i);
      finished = false;
    } else {
      RCLCPP_INFO_STREAM(get_logger(), "servo enable/disable succeed. " << i);
    }
  }

  if (!finished) {
    RCLCPP_ERROR(get_logger(), "[Hardware] servo enable/disable return false. ");
    res->success = false;
  }
#endif
  return true;
}

bool Hardware::init_single_servo(const std::string& joint_name)
{
  RCLCPP_INFO(get_logger(), "[Hardware] init servo[%s]", joint_name.c_str());
  for (auto& dev : devices_) {
    if (dev.type == kEncoderDevice)
      continue;

    if (dev.name == joint_name) {
      auto motor = dev.getDevice<ros_canopen_driver::Motor>();
    
      if (motor) {
          if (!motor->Motor_InitParam()) {
            RCLCPP_ERROR(get_logger(), "[Hardware] motor[%s] param init fail", motor->joint_name.c_str());
            return false;
          }

#ifdef SIMULATION_MODE
          return true;
#endif
          if (motor->Motor_GetState() != ros_canopen_driver::MotorState::Ready) {
              if (motor->Motor_Init()) {
                RCLCPP_ERROR(get_logger(), "[Hardware] motor[%s] init fail", motor->joint_name.c_str());
                return false;
              } else {
                RCLCPP_INFO(get_logger(), "[Hardware] motor[%s] init succeed.", motor->joint_name.c_str());
                return true;
              } 
          } else {
            RCLCPP_INFO(get_logger(), "[Hardware] motor[%s] ready, does not need to init.", motor->joint_name.c_str());
            return true;
          }
      } else {
        RCLCPP_ERROR(get_logger(), "motor[%s] cannot find pointer.", joint_name.c_str());
        return false;
      }
    }
  }

  RCLCPP_ERROR(get_logger(), "[Hardware] InitSingle: motor[%s] cannot find.", joint_name.c_str());
  return false;
}

bool Hardware::halt_single_servo(const std::string& joint_name)
{
  RCLCPP_INFO(get_logger(), "[Hardware] halt servo[%s]", joint_name.c_str());
  for (auto& dev : devices_) {
    if (dev.type == kEncoderDevice)
      continue;
    if (dev.name == joint_name) {
      auto motor = dev.getDevice<ros_canopen_driver::Motor>();
      if (motor) {
          return motor->Motor_Halt();
      } else {
          RCLCPP_ERROR(get_logger(), "motor[%s] cannot find pointer.", joint_name.c_str());
          return false;
      }
    }
  }

  RCLCPP_ERROR(get_logger(), "[Hardware] HaltSingle motor[%s] cannot find.", joint_name.c_str());
  return false;
}

bool Hardware::recover_single_servo(const std::string& joint_name)
{
  RCLCPP_INFO(get_logger(), "[Hardware] recover servo[%s]", joint_name.c_str());
  for (auto& dev : devices_) {
    if (dev.type == kEncoderDevice)
      continue;

    if (dev.name == joint_name) {
      auto motor = dev.getDevice<ros_canopen_driver::Motor>();
    
      if (motor) {
          uint16_t result = 0;
          result |= motor->Motor_GetErrCode();
          result |= (motor->Motor_GetState() != ros_canopen_driver::MotorState::Ready);
          
          if (result != 0) {
              return motor->Motor_Recover(); // 使用 motor 调用 Motor_Recover
          } else {
              RCLCPP_INFO(get_logger(), "[Hardware] servo[%s] does not need to recover", joint_name.c_str());
              return true;
          }
      } else {
          RCLCPP_ERROR(get_logger(), "motor[%s] cannot find pointer.", joint_name.c_str());
          return false;
      }
    }
  }
  RCLCPP_ERROR(get_logger(), "[Hardware] RecoverSingle motor[%s] cannot find.", joint_name.c_str());
  return false;
}

bool Hardware::recover_servos()
{
  assert(servo_.size() > 0);
  std::vector<std::future<bool>> futures;
  futures.resize(0);
  for (auto dev : devices_) {
    if (dev.type == kEncoderDevice)
      continue;
    auto motor = dev.getDevice<ros_canopen_driver::Motor>();
    if (motor == nullptr) {
      RCLCPP_ERROR(get_logger(), "[Hardware] bad servo[%s] pointer. ", dev.name.c_str());
      continue;
    }

    std::future<bool> fut = std::async(std::launch::async, std::bind(&ros_canopen_driver::Motor::Motor_Recover, motor));
    futures.emplace_back(std::move(fut));
  }

  bool result = false;
  bool finished = true;
  for (int i=0; i< futures.size(); i++) {
    try {
      result = futures[i].get();
    }
    catch (const std::future_error& e) {
      RCLCPP_ERROR_STREAM(get_logger(), "[Hardware] future " << i << " get result error: " << e.what());
      finished = false;
    }

    if (!result) {
      RCLCPP_ERROR_STREAM(get_logger(), "servo recover error! " << i);
      finished = false;
    }
    else {
      RCLCPP_INFO_STREAM(get_logger(), "servo recover succeed. " << i);
    }
  }

  if (!finished) {
    RCLCPP_ERROR(get_logger(), "[Hardware] servo recover return false. ");
    agv_msgs::msg::AGVEvent event;
    event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
    event.type = agv_msgs::msg::AGVEvent::ALARM;
    event.action = agv_msgs::msg::AGVEvent::ESTOP;
    event.ackreq = true;
    event.param1 = kEventParamIdRecoverError;
    event.param2 = 0;
    event.description = "[Hardware] servo recover failed.";
    RCLCPP_INFO_STREAM(get_logger(), "[Hardware] Add new event:" << event);
    event_publisher_.add(event, 1, event.param1);
    return false;
  }
  else {
    event_publisher_.clear(1, kEventParamIdRecoverError);
  }

  velocity_interface_running_ = true;
  position_interface_running_ = true;
  return true;
}


bool Hardware::shutdown_servos()
{
  return true;
}

bool Hardware::halt_servos()
{
  assert(servo_.size() > 0);
  velocity_interface_running_ = false;
  position_interface_running_ = false;

  std::vector<std::future<bool>> futures;
  futures.resize(0);
  for (auto dev : devices_) {
    if (dev.type == kEncoderDevice)
      continue;
    auto motor = dev.getDevice<ros_canopen_driver::Motor>();
    if (motor == nullptr) {
      RCLCPP_ERROR(get_logger(), "[Hardware] bad servo[%s] pointer. ", dev.name.c_str());
      continue;
    }

    std::future<bool> fut = std::async(std::launch::async, std::bind(&ros_canopen_driver::Motor::Motor_Halt, motor));
    futures.emplace_back(std::move(fut));
  }

  bool result = false;
  bool finished = true;
  for (int i=0; i< futures.size(); i++) {
    try {
      result = futures[i].get();
    }
    catch (const std::future_error& e) {
      RCLCPP_ERROR_STREAM(get_logger(), "[Hardware] future " << i << " get result error: " << e.what());
      finished = false;
    }

    if (!result) {
      RCLCPP_ERROR_STREAM(get_logger(), "servo init error! " << i);
      finished = false;
    }
    else {
      RCLCPP_INFO_STREAM(get_logger(), "servo init succeed. " << i);
    }
  }

  if (!finished) {
    RCLCPP_ERROR(get_logger(), "[Hardware] servo init return false. ");
    return false;
  }

  velocity_interface_running_ = false;
  position_interface_running_ = false;
  return true;
}

bool Hardware::has_error()
{
  uint16_t result = 0;
  for (auto dev : devices_) {
    if (dev.type == kEncoderDevice)
      continue;

    auto motor = dev.getDevice<ros_canopen_driver::Motor>();
    if (motor) {
      result |= motor->Motor_GetErrCode();
      result |= (motor->Motor_GetState() != ros_canopen_driver::MotorState::Ready);
    } else {
      RCLCPP_ERROR(get_logger(), "motor[%s] cannot find pointer.", dev.name);
      return false;
    }
  }

  return !(result == 0);
}

bool Hardware::call_output_service()
{
  if (outports_.size() > 0) {
    if (output_client_->service_is_ready()) {
      static rclcpp::Time last_call_time = this->now();
      if (!driver_initialized_ || (this->now() - last_call_time).seconds() > 3) {
        auto request = std::make_shared<agv_srvs::srv::OutputInterface::Request>();
        request->index_level = outports_;

        try {
          using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::OutputInterface>::SharedFuture;
          auto response_received_callback = [this](ServiceResponseFuture future) {
            if (future.get()->state)
              RCLCPP_INFO(get_logger(), "call output infterface service success");
            else 
              RCLCPP_WARN(get_logger(), "canopen output infterface service response fail.");
          };

          auto result = output_client_->async_send_request(request, response_received_callback);
        } catch (const std::exception &e) {
          RCLCPP_WARN(get_logger(), "call output infterface service fail.");
        }
      }
      driver_initialized_ = true;
      last_call_time = this->now(); 
    } else {
      RCLCPP_WARN(get_logger(), "output service Not Ready.");
      return false;
    }
  } 
  return true;
}

bool Hardware::call_nmt_service(uint32_t value)
{
  if (set_entry_client_->service_is_ready()) {
    auto request = std::make_shared<agv_srvs::srv::SetCanopenEntry::Request>();
    request->bus_name = std::string("can0");
    request->node_id = 0;
    request->index = 0;
    request->sub_index = 0;
    request->value = value;

    try {
        using ServiceResponseFuture = rclcpp::Client<agv_srvs::srv::SetCanopenEntry>::SharedFuture;
        auto response_received_callback = [this](ServiceResponseFuture future) {
          if (future.get()->success)
            RCLCPP_INFO(get_logger(), "call canopen NMT service success");
          else 
            RCLCPP_WARN(get_logger(), "canopen NMT service response fail.");
        };

        auto result = set_entry_client_->async_send_request(request, response_received_callback);
    } catch (const std::exception &e) {
      RCLCPP_WARN(get_logger(), "call canopen NMT service fail.");
    }
  } else {
    RCLCPP_WARN(get_logger(), "MNT service Not Ready.");
    return false;
  }
  return true;
}

void Hardware::state_publish() 
{
  for (auto &dev : devices_) {
    if (dev.type != kMotorDevice)
      continue;
    auto motor = dev.getDevice<ros_canopen_driver::Motor>(); 
    if (motor != nullptr) {

#ifdef SIMULATION_MODE
      uint16_t errcode = 0; 
#else
      uint16_t errcode = motor->Motor_GetErrCode();
#endif
      int index = dev.index;
      if (errcode) { 
        agv_msgs::msg::AGVEvent event;
        event.ecode = agv_msgs::msg::AGVEvent::CANOPEN_SERVO_ERROR_EVENT;
        event.type = agv_msgs::msg::AGVEvent::ALARM;
        event.action = agv_msgs::msg::AGVEvent::ESTOP;
        event.ackreq = true;
        event.param1 = index;
        event.param2 = errcode;
        event.description = motor->joint_name + ":" + motor->Motor_Err2userErr(errcode);
        event_publisher_.add(event, 1, event.param1);
      } else {
        event_publisher_.clear(1, index);
      }

      driver_status_.at(index).jointname  = motor->joint_name;
#ifdef SIMULATION_MODE
      driver_status_.at(index).statusword = 0x8007;
      driver_status_.at(index).ready = enable_motor_;

      switch (enable_motor_? 
        ros_canopen_driver::MotorState::Ready : ros_canopen_driver::MotorState::Recover
        ) {
#else
      driver_status_.at(index).statusword = motor->Motor_GetStatusWord();
      driver_status_.at(index).ready = motor->Motor_GetState() == ros_canopen_driver::MotorState::Ready;
      switch (motor->Motor_GetState()) {
#endif
        case ros_canopen_driver::MotorState::Off:
          driver_status_.at(index).motorstate = "Off";
          break;
        case ros_canopen_driver::MotorState::Init:
          driver_status_.at(index).motorstate = "Init";
          break;
        case ros_canopen_driver::MotorState::Shutdown:
          driver_status_.at(index).motorstate = "Shutdown";
          break;
        case ros_canopen_driver::MotorState::Error:
          driver_status_.at(index).motorstate = "Error";
          break;
        case ros_canopen_driver::MotorState::Halt:
          driver_status_.at(index).motorstate = "Halt";
          break;
        case ros_canopen_driver::MotorState::Recover:
          driver_status_.at(index).motorstate = "Recover";
          break;
        case ros_canopen_driver::MotorState::Ready:
          driver_status_.at(index).motorstate = "Ready";
          break;
      }
    }
  }
  drivers_info_.drivers = driver_status_;
  drivers_info_publisher_->publish(drivers_info_);
}

void Hardware::quit()
{
  for (auto dev : devices_) {
    // TODO: add encoder operate for sddriver; add lift_monitor.stop();
    auto motor = dev.getDevice<ros_canopen_driver::Motor>();
    if (dev.type == kMotorDevice && motor ) {
        motor->Motor_Shutdown();
    }
  }
}

bool Hardware::init_servos()
{
  while (rclcpp::ok) {
    bool all_initialized = true;
    for (auto& dev : devices_) {
      if (dev.type == kEncoderDevice)
        continue;
     
      auto motor = dev.getDevice<ros_canopen_driver::Motor>();
      if (motor->servo_manufacturer < 20) {
        bool can_initialize = false;
        
        // find all device start seq 
        if (dev.start_seq.empty()) {
          RCLCPP_INFO(get_logger(), "start without wait other device");
          can_initialize = true; 
        } else {
          RCLCPP_INFO(get_logger(), 
              "first waiting for one device (%s) to initialize, then proceeding to activate the second device (%s)", 
              dev.start_seq.c_str(), dev.name.c_str());
          auto dep_dev = std::find_if(devices_.begin(), devices_.end(),
              [&dev](const HWDevice& d) { return d.name == dev.start_seq; });
          if (dep_dev != devices_.end() && dep_dev->initialized) {
            RCLCPP_INFO(get_logger(), "aready start %s then start %s", dev.start_seq.c_str(), dev.name.c_str());
            can_initialize = true; 
          } else if (dep_dev == devices_.end()) {  
            RCLCPP_WARN(get_logger(), "can't find %s after %s", dev.name.c_str(), dev.start_seq.c_str());
            can_initialize = true;
          }
        }

        if (can_initialize) {
          if (!dev.initialized && init_futures_.find(dev.name) == init_futures_.end()) {
            RCLCPP_INFO(get_logger(), "futures add servo[%s] type[%d]", dev.name.c_str(), dev.type);
            init_futures_[dev.name] = std::async(std::launch::async, std::bind(
              &Hardware::init_single_servo, this, std::placeholders::_1), dev.name);   
          }

          if (!dev.initialized && init_futures_.find(dev.name) != init_futures_.end()) {
            std::future_status status = init_futures_[dev.name].wait_for(std::chrono::seconds(0));
            if (status == std::future_status::ready) {
              bool result = false;
              try {
                result = init_futures_[dev.name].get();
              } catch (const std::future_error& e) {
                RCLCPP_ERROR_STREAM(get_logger(), "future for servo[" << dev.name << "] get result error: " << e.what());
                result = false;
              }

              if (result) {
                RCLCPP_INFO(get_logger(), "servo[%s] init succeed.", dev.name.c_str());
                dev.initialized = true;
              }
              init_futures_.erase(dev.name);
            }
          }
        } 
      }
      if(!dev.initialized)
        all_initialized = false;
    }
    if (all_initialized) {
      RCLCPP_INFO(get_logger(), "All devices initialized successfully!");
      return true;
    }
    rclcpp::sleep_for(std::chrono::milliseconds(100));
  }
  return true;
}

bool Hardware::handle_init(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                   std::shared_ptr<std_srvs::srv::Trigger::Response> res)
{
  RCLCPP_INFO(this->get_logger(), "Initializing XXX");
  // 执行初始化
  res->success = init_servos();
  res->message = "";
  return true;
}

bool Hardware::handle_recover(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                    std::shared_ptr<std_srvs::srv::Trigger::Response> res)
{
  if (driver_initialized_) {
      RCLCPP_INFO(this->get_logger(), "Recover servo");
      // 执行恢复
      res->success = recover_servos();
      res->message = "";
      return true;
  } else {
      res->success = true;
      return true;
  }
}

bool Hardware::handle_shutdown(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                     std::shared_ptr<std_srvs::srv::Trigger::Response> res)
{
  if (driver_initialized_) {
      RCLCPP_INFO(this->get_logger(), "Shutdown XXX");
      // 执行关机
      res->success = shutdown_servos();
      res->message = "";
      return true;
  } else {
      res->success = true;
      return true;
  }
}

bool Hardware::handle_halt(const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
                 std::shared_ptr<std_srvs::srv::Trigger::Response> res)
{
  if (driver_initialized_) {
      RCLCPP_INFO(this->get_logger(), "Halt XXX");
      // 执行停止
      res->success = halt_servos();
      res->message = "";
      return true;
  } else {
      res->success = true;
      return true;
  }
}

void Hardware::reset_button_callback(const agv_msgs::msg::ReadInPutsNew::SharedPtr msg)
{
  int new_reset_level = 0;
  std::vector<agv_msgs::msg::PairTypeInt> input_msg = msg->array;
  for (const auto &data : input_msg) {
      if (data.key.compare(0, 15, "emergencyButton") == 0) {
          if (data.value == 1) {
              return;
          }
      }
  }
  for (const auto &data : input_msg) {
      if (data.key.compare("resetButton") == 0) {
          new_reset_level = data.value;
      }
  }
  if (new_reset_level != reset_level_ && new_reset_level == 1) {
      event_publisher_.clear();
      // if (hardware_interface_->hasError() && driver_initialized_) {
      //     hardware_interface_->servoRecover();
      // }
     call_nmt_service(1);
  }
  reset_level_ = new_reset_level;
}

}  // namespace hardware
